Rotation¶

Euler angles¶

Extrinsic Rotation¶

Right multiply matrix

In [1]:
import sympy
from sympy.vector import AxisOrienter, CoordSys3D
x, y, theta = sympy.symbols("x y theta")
N = CoordSys3D("N")
M = AxisOrienter(theta, N.k).rotation_matrix(N)
M
Out[1]:
$\displaystyle \left[\begin{matrix}\cos{\left(\theta \right)} & \sin{\left(\theta \right)} & 0\\- \sin{\left(\theta \right)} & \cos{\left(\theta \right)} & 0\\0 & 0 & 1\end{matrix}\right]$
In [2]:
M.inv()
Out[2]:
$\displaystyle \left[\begin{matrix}- \frac{\sin^{2}{\left(\theta \right)}}{\cos{\left(\theta \right)}} + \frac{1}{\cos{\left(\theta \right)}} & - \sin{\left(\theta \right)} & 0\\\sin{\left(\theta \right)} & \cos{\left(\theta \right)} & 0\\0 & 0 & 1\end{matrix}\right]$
In [3]:
v = sympy.Matrix([x, y, 0]).T * M.subs(theta, sympy.pi/2)
v
Out[3]:
$\displaystyle \left[\begin{matrix}- y & x & 0\end{matrix}\right]$
In [4]:
import py3d
py3d.Transform.from_euler("xyz", [0.4, -0.2, 0])

Welcome to py3d world, please visit https://tumiz.github.io/scenario/ for more information
Out[4]:
Transform([[ 0.98006658,  0.        ,  0.19866933,  0.        ],
           [-0.07736548,  0.92106099,  0.3816559 ,  0.        ],
           [-0.18298657, -0.38941834,  0.9027011 ,  0.        ],
           [ 0.        ,  0.        ,  0.        ,  1.        ]])

Intrinsic Rotation¶

In [5]:
import py3d
py3d.Transform.from_euler("XYZ", [0.4, -0.2, 0])
Out[5]:
Transform([[ 0.98006658, -0.07736548,  0.18298657,  0.        ],
           [ 0.        ,  0.92106099,  0.38941834,  0.        ],
           [-0.19866933, -0.3816559 ,  0.9027011 ,  0.        ],
           [ 0.        ,  0.        ,  0.        ,  1.        ]])
In [6]:
import py3d
py3d.Transform.from_rpy([1.3, 1.0, 0]) @ py3d.Transform.from_rpy([0.9, .0, 0])
Out[6]:
Transform([[ 0.54030231,  0.68032627,  0.49520661,  0.        ],
           [ 0.        , -0.58850112,  0.8084964 ,  0.        ],
           [ 0.84147098, -0.43683247, -0.31796851,  0.        ],
           [ 0.        ,  0.        ,  0.        ,  1.        ]])

Apply a intrinsic rotation to a point

In [8]:
import py3d
p = py3d.Vector3([1, 3, 4])
py3d.label("P", p.tolist())
py3d.render(p.as_point(py3d.Color(g=1)))
t = py3d.Transform.from_euler("XYZ", [0, 0, 1])
p1 = p @ t
a = py3d.axis()
a.vertex @= t
py3d.label("P1", p1.tolist())
py3d.render(p1.as_point(py3d.Color(r=1)), a)
py3d.grid()
Out[8]:

Difference between extrinsic rotation and intrinsic rotation¶

In [9]:
import py3d
e_car = py3d.car()
e_car.color = py3d.Color(g=1)
e_car.vertex @= py3d.Transform.from_euler("xyz", [0.4, -0.2, 0])
py3d.label("Extrincs", [4, 1, 3], color="green")
i_car = py3d.car()
i_car.color = py3d.Color(r=1)
i_car.vertex @= py3d.Transform.from_euler("XYZ", [0.4, -0.2, 0])
py3d.label("Intrincs", [4, -1, 2], color="red")
py3d.render(e_car, i_car)
py3d.grid(step=2)
Out[9]:

RPY¶

In py3d, rpy represents roll, pitch and yaw, it is kind of intrinsic rotation

In [10]:
from PIL import Image
import numpy
import py3d
img = numpy.array(Image.open("py3d/doc/20220917214012.jpg"))
hw = img.shape[0]//2
hh = img.shape[1]//2
point = py3d.Vector3.grid(range(-hw, hw), range(-hh, hh))[::3, ::3]*0.01
point = point.as_point()
point.color = py3d.Vector3(img/255).H[::3, ::3]
point.vertex @= py3d.Transform.from_rpy([0, 0, -py3d.pi/2])
point
Out[10]:

Axis angle¶

Axis angle rotation matrix¶

In [11]:
from sympy.vector import AxisOrienter, CoordSys3D
import sympy
x, y, z, theta = sympy.symbols("x y z theta")
N = CoordSys3D('N')
expr = AxisOrienter(theta, x*N.i+y*N.j+z*N.k).rotation_matrix(N)
expr.subs(x**2+y**2+z**2, 1)
Out[11]:
$\displaystyle \left[\begin{matrix}x^{2} + \left(1 - x^{2}\right) \cos{\left(\theta \right)} & - x y \cos{\left(\theta \right)} + x y + z \sin{\left(\theta \right)} & - x z \cos{\left(\theta \right)} + x z - y \sin{\left(\theta \right)}\\- x y \cos{\left(\theta \right)} + x y - z \sin{\left(\theta \right)} & y^{2} + \left(1 - y^{2}\right) \cos{\left(\theta \right)} & x \sin{\left(\theta \right)} - y z \cos{\left(\theta \right)} + y z\\- x z \cos{\left(\theta \right)} + x z + y \sin{\left(\theta \right)} & - x \sin{\left(\theta \right)} - y z \cos{\left(\theta \right)} + y z & z^{2} + \left(1 - z^{2}\right) \cos{\left(\theta \right)}\end{matrix}\right]$
In [12]:
import py3d
py3d.Transform.from_axis_angle(axis=[0, 0, 1], angle=[1, 2])
Out[12]:
Transform([[[ 0.54030231,  0.84147098,  0.        ,  0.        ],
            [-0.84147098,  0.54030231,  0.        ,  0.        ],
            [ 0.        ,  0.        ,  1.        ,  0.        ],
            [ 0.        ,  0.        ,  0.        ,  1.        ]],

           [[-0.41614684,  0.90929743,  0.        ,  0.        ],
            [-0.90929743, -0.41614684,  0.        ,  0.        ],
            [ 0.        ,  0.        ,  1.        ,  0.        ],
            [ 0.        ,  0.        ,  0.        ,  1.        ]]])

Order of rotation and translation

In [13]:
import sympy
r = sympy.symbols("r:9")
dr = sympy.symbols("dr:9")
t = sympy.symbols("t:3")
R = sympy.Matrix([
    [r[0], r[1], r[2], 0],
    [r[3], r[4], r[5], 0],
    [r[6], r[7], r[8], 0],
    [0, 0, 0, 1]
])
T = sympy.Matrix([
    [1, 0, 0, 0],
    [0, 1, 0, 0],
    [0, 0, 1, 0],
    [t[0], t[1], t[2], 1]
])
R*T
Out[13]:
$\displaystyle \left[\begin{matrix}r_{0} & r_{1} & r_{2} & 0\\r_{3} & r_{4} & r_{5} & 0\\r_{6} & r_{7} & r_{8} & 0\\t_{0} & t_{1} & t_{2} & 1\end{matrix}\right]$

Get rotation matrix from two vectors

In [14]:
import py3d
py3d.Transform.from_two_vectors([1, 0, 0], [0, 0, 1]).as_rpy()

/home/runner/work/scenario/scenario/py3d/py3d/core.py:630: RuntimeWarning: invalid value encountered in arcsin
  ret[..., i[1]] = getattr(numpy, 'arccos' if m[0] == m[2] else 'arcsin')(
Out[14]:
Vector3([3.14159265,        nan, 3.14159265])

Rotation Vector¶

In [15]:
import py3d
py3d.Transform.from_rotation_vector([1, 2, 3])
Out[15]:
Transform([[-0.69492056, -0.19200697,  0.69297817,  0.        ],
           [ 0.71352099, -0.30378504,  0.6313497 ,  0.        ],
           [ 0.08929286,  0.93319235,  0.34810748,  0.        ],
           [ 0.        ,  0.        ,  0.        ,  1.        ]])

Quaternion¶

In py3d, quaternion is in scalar-last format. Normaly, Vector4 is used to represent it.

In [17]:
import py3d
py3d.Vector4(x=0, y=0, z=0, w=1)
Out[17]:
Vector4([0., 0., 0., 1.])
In [18]:
import py3d
py3d.Transform.from_quaternion([0, 0, 0, 1])
Out[18]:
Transform([[1., 0., 0., 0.],
           [0., 1., 0., 0.],
           [0., 0., 1., 0.],
           [0., 0., 0., 1.]])

Get a quaternion from an axis angle pair, and get its rotation matrix

In [19]:
import py3d
angle_axis = py3d.Vector4(x=1, y=1, z=1, w=py3d.pi/2)
print(angle_axis)
q = angle_axis.from_axis_angle_to_quaternion()
print(q)
py3d.Transform.from_quaternion(q)

[1.         1.         1.         1.57079633]
[0.40824829 0.40824829 0.40824829 0.70710678]
Out[19]:
Transform([[ 0.33333333,  0.9106836 , -0.24401694,  0.        ],
           [-0.24401694,  0.33333333,  0.9106836 ,  0.        ],
           [ 0.9106836 , -0.24401694,  0.33333333,  0.        ],
           [ 0.        ,  0.        ,  0.        ,  1.        ]])
In [20]:
import py3d
q = [
    [0, 0, 0, 1],
    [0, 0, 1, 0],
    [0, 1, 0, 0],
    [1, 0, 0, 0]
]
py3d.Transform.from_quaternion(q)
Out[20]:
Transform([[[ 1.,  0.,  0.,  0.],
            [ 0.,  1.,  0.,  0.],
            [ 0.,  0.,  1.,  0.],
            [ 0.,  0.,  0.,  1.]],

           [[-1.,  0.,  0.,  0.],
            [ 0., -1.,  0.,  0.],
            [ 0.,  0.,  1.,  0.],
            [ 0.,  0.,  0.,  1.]],

           [[-1.,  0.,  0.,  0.],
            [ 0.,  1.,  0.,  0.],
            [ 0.,  0., -1.,  0.],
            [ 0.,  0.,  0.,  1.]],

           [[ 1.,  0.,  0.,  0.],
            [ 0., -1.,  0.,  0.],
            [ 0.,  0., -1.,  0.],
            [ 0.,  0.,  0.,  1.]]])

Convert quaternion to angle axis

In [22]:
import py3d
quat = py3d.Transform.from_quaternion(
    [0.18257419, 0.36514837, 0.54772256, 0.73029674])
quat.as_axis_angle()
Out[22]:
Vector4([0.26726125, 0.53452248, 0.80178373, 1.50408018])

Convert RPY to quaternion¶

Return quaternion with sequence "xyzw"

In [24]:
import py3d
rpy = [
    [0, 0, -py3d.pi],
    [0, 0, -py3d.pi-0.01],
    [0, 0, -py3d.pi/3],
    [-1, 0, 0],
    [-py3d.pi, 0, 1],
    [0, -py3d.pi, 0]]
py3d.Transform.from_rpy(rpy).as_quaternion()
Out[24]:
Vector4([[ 0.        ,  0.        , -1.        ,  0.        ],
         [ 0.        ,  0.        ,  0.9999875 ,  0.00499998],
         [ 0.        ,  0.        , -0.5       ,  0.8660254 ],
         [-0.47942554,  0.        ,  0.        ,  0.87758256],
         [-0.87758256,  0.47942554,  0.        ,  0.        ],
         [ 0.        , -1.        ,  0.        ,  0.        ]])
In [26]:
import py3d
# positions
p = [[3.24544068, -1.4610586,  1.21331756],
     [3.66378017, -1.32072563,  1.89712674],
     [-5.0884622, -2.48626808,  1.68773464],
     [-5.47338134, -2.65634697,  2.02463642],
     [1.830746, -0.8155359,  1.90245186],
     [-0.7094184, -0.84719837,  1.4467056],
     [-1.72178753, -0.681502,  1.17706321],
     [-3.88463547, -1.20610078,  1.14303617],
     [-4.527405, -3.12150274,  0.94426914],
     [4.13260871, -1.71061771,  1.49295544],
     [3.25896384, -1.46451182,  0.9032174],
     [-3.63891521, -1.03317465,  1.11405222]]
# quaternions
q = [[0.00307048, -0.27852711, -0.24115858,  0.92965357],
     [0.00955487, -0.328664, -0.25972646,  0.90798174],
     [0.05519327,  0.22308439,  0.22751421,  0.94626864],
     [0.05064761,  0.24596963,  0.23314524,  0.93945572],
     [-0.01006987, -0.17448035, -0.11591101,  0.97776267],
     [0.01230182, -0.03710485,  0.00123949,  0.99923489],
     [0.02991609,  0.0383105,  0.0729396,  0.99615117],
     [0.05252438,  0.12527874,  0.1242716,  0.98290538],
     [-0.09333274,  0.14651227,  0.2808575,  0.94389735],
     [0.00967634, -0.29085732, -0.28211318,  0.91417752],
     [0.00214324, -0.25691119, -0.23230781,  0.93809655],
     [0.04813863,  0.1177435,  0.11584668,  0.98508816]]

vertice = py3d.Vector3(
    z=[0, 1]) @ py3d.Transform.from_quaternion(q) @ py3d.Transform.from_translation(p)
directions = vertice.as_linesegment()
directions.start.color = py3d.Color(b=1)
directions.end.color = py3d.Color(r=1)
py3d.render(directions)
py3d.grid()
Out[26]:

↑Top

←Transform

←Home